
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_imu.c
  * @author     baiyang
  * @date       2021-10-13
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include <string.h>

#include "sensor_imu.h"
#include  "sensor_imu_invensense.h"

#include <rtthread.h>

#include <mavproxy/mavproxy.h>
#include <device_manager/dev_mgr.h>
#include <common/console/console.h>
#include <board_config/borad_config.h>
/*-----------------------------------macro------------------------------------*/
#define SENSOR_IMU_GCS_SEND_TEXT(severity, format, args...) mavproxy_send_statustext(severity, format, ##args)

/* Define INS_TIMING_DEBUG to track down scheduling issues with the main loop.
 * Output is on the debug console. */
#ifdef INS_TIMING_DEBUG
#include <stdio.h>
#define timing_printf(fmt, args...)      do { printf("[timing] " fmt, ##args); } while(0)
#else
#define timing_printf(fmt, args...)
#endif

#ifndef HAL_DEFAULT_INS_FAST_SAMPLE
#define HAL_DEFAULT_INS_FAST_SAMPLE 1
#endif

#define DEFAULT_GYRO_FILTER  20
#define DEFAULT_ACCEL_FILTER 20
#define DEFAULT_STILL_THRESH 0.1f

#if defined(STM32H7) || defined(STM32F7)
#define MPU_FIFO_FASTSAMPLE_DEFAULT 1
#else
#define MPU_FIFO_FASTSAMPLE_DEFAULT 0
#endif

#define GYRO_INIT_MAX_DIFF_DPS 0.1f

#ifndef HAL_INS_TRIM_LIMIT_DEG
#define HAL_INS_TRIM_LIMIT_DEG 10
#endif

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void _init_gyro();
static bool _calculate_trim(const Vector3f_t *accel_sample, Vector3f_t *trim);
float sensor_imu_get_delta_time();
bool sensor_imu_get_gyro_health2(uint8_t instance);
bool sensor_imu_get_gyro_health(void);
bool sensor_imu_get_accel_health2(uint8_t instance);
bool sensor_imu_get_accel_health(void);
/*----------------------------------variable----------------------------------*/
static sensor_imu _sensor_imu;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void sensor_imu_ctor()
{
    sensor_imu_update_param();

    _sensor_imu._board_orientation = ROTATION_NONE;

    for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
        _sensor_imu._gyro_cal_ok[i] = true;
        _sensor_imu._accel_max_abs_offsets[i] = 3.5f;
    }

    for (uint8_t i=0; i<INS_VIBRATION_CHECK_INSTANCES; i++) {
        lpf_set_cutoff1_vec3f(&(_sensor_imu._accel_vibe_floor_filter[i]), AP_INERTIAL_SENSOR_ACCEL_VIBE_FLOOR_FILT_HZ);
        lpf_set_cutoff1_vec3f(&(_sensor_imu._accel_vibe_filter[i]), AP_INERTIAL_SENSOR_ACCEL_VIBE_FILT_HZ);
    }

    _sensor_imu._backend_count = 0;
}

/*
 * Get the AP_InertialSensor singleton
 */
sensor_imu *sensor_imu_get_singleton()
{
    return &_sensor_imu;
}

uint8_t sensor_imu_get_primary_accel(void) { return _sensor_imu._primary_accel; }
uint8_t sensor_imu_get_primary_gyro(void) { return _sensor_imu._primary_gyro; }

/*
  register a new gyro instance
 */
bool sensor_imu_register_gyro(uint8_t *instance, uint16_t raw_sample_rate_hz, uint32_t id)
{
    if (_sensor_imu._gyro_count == INS_MAX_INSTANCES) {
        SENSOR_IMU_GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Failed to register gyro id %u", id);
        return false;
    }

    _sensor_imu._gyro_raw_sample_rates[_sensor_imu._gyro_count] = raw_sample_rate_hz;
    _sensor_imu._gyro_over_sampling[_sensor_imu._gyro_count] = 1;
    _sensor_imu._gyro_raw_sampling_multiplier[_sensor_imu._gyro_count] = INT16_MAX/radians(2000);

    if ((uint32_t)_sensor_imu._gyro_id[_sensor_imu._gyro_count] != id) {
        // inconsistent gyro id - mark it as needing calibration
        _sensor_imu._gyro_cal_ok[_sensor_imu._gyro_count] = false;
    }

    _sensor_imu._gyro_id[_sensor_imu._gyro_count] = id;

    *instance = _sensor_imu._gyro_count++;

    return true;
}

/*
  register a new accel instance
 */
bool sensor_imu_register_accel(uint8_t *instance, uint16_t raw_sample_rate_hz, uint32_t id)
{
    if (_sensor_imu._accel_count == INS_MAX_INSTANCES) {
        SENSOR_IMU_GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Failed to register accel id %u", id);
        return false;
    }

    _sensor_imu._accel_raw_sample_rates[_sensor_imu._accel_count] = raw_sample_rate_hz;
    _sensor_imu._accel_over_sampling[_sensor_imu._accel_count] = 1;
    _sensor_imu._accel_raw_sampling_multiplier[_sensor_imu._accel_count] = INT16_MAX/(16*GRAVITY_MSS);

    if ((uint32_t)_sensor_imu._accel_id[_sensor_imu._accel_count] != id) {
        // inconsistent accel id
        _sensor_imu._accel_id_ok[_sensor_imu._accel_count] = false;
    } else {
        _sensor_imu._accel_id_ok[_sensor_imu._accel_count] = true;
    }

    _sensor_imu._accel_id[_sensor_imu._accel_count] = (int32_t)id;

    *instance = _sensor_imu._accel_count++;
    return true;
}

/*
 * Start all backends for gyro and accel measurements. It automatically calls
 * detect_backends() if it has not been called already.
 */
void sensor_imu_start_backends()
{
    sensor_imu_detect_backends();

    for (uint8_t i = 0; i < _sensor_imu._backend_count; i++) {
        sensor_imu_backend_start(_sensor_imu._backends[i]);
    }

    if (_sensor_imu._gyro_count == 0 || _sensor_imu._accel_count == 0) {
        console_panic("INS needs at least 1 gyro and 1 accel, at function:%s, line number:%d",__FUNCTION__, __LINE__);
    }

    // clear IDs for unused sensor instances
    for (uint8_t i=_sensor_imu._accel_count; i<INS_MAX_INSTANCES; i++) {
        _sensor_imu._accel_id[i] = 0;
    }
    for (uint8_t i=_sensor_imu._gyro_count; i<INS_MAX_INSTANCES; i++) {
        _sensor_imu._gyro_id[i] = 0;
    }
}

/*
  see if gyro calibration should be performed
 */
enum Gyro_Calibration_Timing sensor_imu_gyro_calibration_timing()
{
    return (enum Gyro_Calibration_Timing)_sensor_imu._gyro_cal_timing;
}

void sensor_imu_init(uint16_t loop_rate)
{
    // remember the sample rate
    _sensor_imu._loop_rate = loop_rate;
    _sensor_imu._loop_delta_t = 1.0f / loop_rate;

    // we don't allow deltat values greater than 10x the normal loop
    // time to be exposed outside of INS. Large deltat values can
    // cause divergence of state estimators
    _sensor_imu._loop_delta_t_max = 10 * _sensor_imu._loop_delta_t;

    if (_sensor_imu._gyro_count == 0 && _sensor_imu._accel_count == 0) {
        sensor_imu_start_backends();
    }

    // initialise accel scale if need be. This is needed as we can't
    // give non-zero default values for vectors in AP_Param
    for (uint8_t i=0; i<_sensor_imu._accel_count; i++) {
        if (vec3_is_zero(&(_sensor_imu._accel_scale[i]))) {
            _sensor_imu._accel_scale[i].x = 1.0f;
            _sensor_imu._accel_scale[i].y = 1.0f;
            _sensor_imu._accel_scale[i].z = 1.0f;
        }
    }

    // calibrate gyros unless gyro calibration has been disabled
    if (sensor_imu_gyro_calibration_timing() != GYRO_CAL_NEVER) {
        sensor_imu_init_gyro();
    }

    _sensor_imu._sample_period_usec = 1000*1000UL / _sensor_imu._loop_rate;

    // establish the baseline time between samples
    _sensor_imu._delta_time = 0;
    _sensor_imu._next_sample_usec = 0;
    _sensor_imu._last_sample_usec = 0;
    _sensor_imu._have_sample = false;

#if 0
    // initialise IMU batch logging
    batchsampler.init();

    // the center frequency of the harmonic notch is always taken from the calculated value so that it can be updated
    // dynamically, the calculated value is always some multiple of the configured center frequency, so start with the
    // configured value
    _calculated_harmonic_notch_freq_hz[0] = _harmonic_notch_filter.center_freq_hz();
    _num_calculated_harmonic_notch_frequencies = 1;

    for (uint8_t i=0; i<get_gyro_count(); i++) {
        _gyro_harmonic_notch_filter[i].allocate_filters(_harmonic_notch_filter.harmonics(), _harmonic_notch_filter.hasOption(HarmonicNotchFilterParams::Options::DoubleNotch));
        // initialise default settings, these will be subsequently changed in AP_InertialSensor_Backend::update_gyro()
        _gyro_harmonic_notch_filter[i].init(_gyro_raw_sample_rates[i], _calculated_harmonic_notch_freq_hz[0],
             _harmonic_notch_filter.bandwidth_hz(), _harmonic_notch_filter.attenuation_dB());
    }
#endif
}

// save parameters to eeprom
void sensor_imu_save_gyro_calibration()
{
    for (uint8_t i=_sensor_imu._gyro_count; i<INS_MAX_INSTANCES; i++) {
        vec3_zero(&(_sensor_imu._gyro_offset[i]));
        _sensor_imu._gyro_id[i] = 0;
    }

    sensor_imu_gyr_offset_id_variable_to_param();
}

// return true if we are in a calibration
bool sensor_imu_calibrating()
{
    return _sensor_imu._calibrating_accel || _sensor_imu._calibrating_gyro;
}

void sensor_imu_init_gyro()
{
    _init_gyro();

    // save calibration
    sensor_imu_save_gyro_calibration();
}

/*
  update gyro and accel values from backends
 */
void sensor_imu_update(void)
{
    // during initialisation update() may be called without
    // wait_for_sample(), and a wait is implied
    sensor_imu_wait_for_sample();

    for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
        // mark sensors unhealthy and let update() in each backend
        // mark them healthy via _publish_gyro() and
        // _publish_accel()
        _sensor_imu._gyro_healthy[i] = false;
        _sensor_imu._accel_healthy[i] = false;
        _sensor_imu._delta_velocity_valid[i] = false;
        _sensor_imu._delta_angle_valid[i] = false;
    }
    for (uint8_t i=0; i<_sensor_imu._backend_count; i++) {
        sensor_imu_backend_update(_sensor_imu._backends[i]);
    }

    if (!_sensor_imu._startup_error_counts_set) {
        for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
            _sensor_imu._accel_startup_error_count[i] = _sensor_imu._accel_error_count[i];
            _sensor_imu._gyro_startup_error_count[i] = _sensor_imu._gyro_error_count[i];
        }

        if (_sensor_imu._startup_ms == 0) {
            _sensor_imu._startup_ms = time_millis();
        } else if (time_millis()-_sensor_imu._startup_ms > 2000) {
            _sensor_imu._startup_error_counts_set = true;
        }
    }

    for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
        if (_sensor_imu._accel_error_count[i] < _sensor_imu._accel_startup_error_count[i]) {
            _sensor_imu._accel_startup_error_count[i] = _sensor_imu._accel_error_count[i];
        }
        if (_sensor_imu._gyro_error_count[i] < _sensor_imu._gyro_startup_error_count[i]) {
            _sensor_imu._gyro_startup_error_count[i] = _sensor_imu._gyro_error_count[i];
        }
    }

    // adjust health status if a sensor has a non-zero error count
    // but another sensor doesn't.
    bool have_zero_accel_error_count = false;
    bool have_zero_gyro_error_count = false;
    for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
        if (_sensor_imu._accel_healthy[i] && _sensor_imu._accel_error_count[i] <= _sensor_imu._accel_startup_error_count[i]) {
            have_zero_accel_error_count = true;
        }
        if (_sensor_imu._gyro_healthy[i] && _sensor_imu._gyro_error_count[i] <= _sensor_imu._gyro_startup_error_count[i]) {
            have_zero_gyro_error_count = true;
        }
    }

    for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
        if (_sensor_imu._gyro_healthy[i] && _sensor_imu._gyro_error_count[i] > _sensor_imu._gyro_startup_error_count[i] && have_zero_gyro_error_count) {
            // we prefer not to use a gyro that has had errors
            _sensor_imu._gyro_healthy[i] = false;
        }
        if (_sensor_imu._accel_healthy[i] && _sensor_imu._accel_error_count[i] > _sensor_imu._accel_startup_error_count[i] && have_zero_accel_error_count) {
            // we prefer not to use a accel that has had errors
            _sensor_imu._accel_healthy[i] = false;
        }
    }

    // set primary to first healthy accel and gyro
    for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
        if (_sensor_imu._gyro_healthy[i] && _sensor_imu._use[i]) {
            _sensor_imu._primary_gyro = i;
            break;
        }
    }
    for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
        if (_sensor_imu._accel_healthy[i] && _sensor_imu._use[i]) {
            _sensor_imu._primary_accel = i;
            break;
        }
    }

    _sensor_imu._last_update_usec = time_micros();
    
    _sensor_imu._have_sample = false;
}

/*
  wait for a sample to be available. This is the function that
  determines the timing of the main loop in ardupilot.

  Ideally this function would return at exactly the rate given by the
  sample_rate argument given to AP_InertialSensor::init().

  The key output of this function is _delta_time, which is the time
  over which the gyro and accel integration will happen for this
  sample. We want that to be a constant time if possible, but if
  delays occur we need to cope with them. The long term sum of
  _delta_time should be exactly equal to the wall clock elapsed time
 */
void sensor_imu_wait_for_sample(void)
{
    if (_sensor_imu._have_sample) {
        // the user has called wait_for_sample() again without
        // consuming the sample with update()
        return;
    }

    uint32_t now = time_micros();

    if (_sensor_imu._next_sample_usec == 0 && _sensor_imu._delta_time <= 0) {
        // this is the first call to wait_for_sample()
        _sensor_imu._last_sample_usec = now - _sensor_imu._sample_period_usec;
        _sensor_imu._next_sample_usec = now + _sensor_imu._sample_period_usec;
        goto check_sample;
    }

    // see how long it is till the next sample is due
    if (_sensor_imu._next_sample_usec - now <=_sensor_imu._sample_period_usec) {
        // we're ahead on time, schedule next sample at expected period
        uint32_t wait_usec = _sensor_imu._next_sample_usec - now;
        time_delay_us(wait_usec);
        uint32_t now2 = time_micros();
        if (now2+100 < _sensor_imu._next_sample_usec) {
            timing_printf("shortsleep %u\n", (unsigned)_sensor_imu._next_sample_usec-now2);
        }
        if (now2 > _sensor_imu._next_sample_usec+400) {
            timing_printf("longsleep %u wait_usec=%u\n",
                          (unsigned)(now2-_sensor_imu._next_sample_usec),
                          (unsigned)wait_usec);
        }
        _sensor_imu._next_sample_usec += _sensor_imu._sample_period_usec;
    } else if (now - _sensor_imu._next_sample_usec < _sensor_imu._sample_period_usec/8) {
        // we've overshot, but only by a small amount, keep on
        // schedule with no delay
        timing_printf("overshoot1 %u\n", (unsigned)(now-_sensor_imu._next_sample_usec));
        _sensor_imu._next_sample_usec += _sensor_imu._sample_period_usec;
    } else {
        // we've overshot by a larger amount, re-zero scheduling with
        // no delay
        timing_printf("overshoot2 %u\n", (unsigned)(now-_sensor_imu._next_sample_usec));
        _sensor_imu._next_sample_usec = now + _sensor_imu._sample_period_usec;
    }

        // now we wait until we have the gyro and accel samples we need
        uint8_t gyro_available_mask = 0;
        uint8_t accel_available_mask = 0;
        uint32_t wait_counter = 0;
        // allow to wait for up to 1/3 of the loop time for samples from all
        // IMUs to come in
        const uint8_t wait_per_loop = 100;
        const uint8_t wait_counter_limit = (uint32_t)(_sensor_imu._loop_delta_t * 1.0e6) / (3*wait_per_loop);
check_sample:

        while (true) {
            for (uint8_t i=0; i<_sensor_imu._backend_count; i++) {
                // this is normally a nop, but can be used by backends
                // that don't accumulate samples on a timer
                sensor_imu_backend_accumulate(_sensor_imu._backends[i]);
            }

            for (uint8_t i=0; i<_sensor_imu._gyro_count; i++) {
                if (_sensor_imu._new_gyro_data[i]) {
                    const uint8_t imask = (1U<<i);
                    gyro_available_mask |= imask;
                    if (_sensor_imu._use[i]) {
                        _sensor_imu._gyro_wait_mask |= imask;
                    } else {
                        _sensor_imu._gyro_wait_mask &= ~imask;
                    }
                }
            }
            for (uint8_t i=0; i<_sensor_imu._accel_count; i++) {
                if (_sensor_imu._new_accel_data[i]) {
                    const uint8_t imask = (1U<<i);
                    accel_available_mask |= imask;
                    if (_sensor_imu._use[i]) {
                        _sensor_imu._accel_wait_mask |= imask;
                    } else {
                        _sensor_imu._accel_wait_mask &= ~imask;
                    }
                }
            }

            // we wait for up to 1/3 of the loop time to get all of the required
            // accel and gyro samples. After that we accept at least
            // one of each
            if (wait_counter < wait_counter_limit) {
                if (gyro_available_mask &&
                    ((gyro_available_mask & _sensor_imu._gyro_wait_mask) == _sensor_imu._gyro_wait_mask) &&
                    accel_available_mask &&
                    ((accel_available_mask & _sensor_imu._accel_wait_mask) == _sensor_imu._accel_wait_mask)) {
                    break;
                }
            } else {
                if (gyro_available_mask && accel_available_mask) {
                    // reset the wait mask so we don't keep delaying
                    // for a dead IMU on the next loop. As soon as it
                    // comes back we will start waiting on it again
                    _sensor_imu._gyro_wait_mask &= gyro_available_mask;
                    _sensor_imu._accel_wait_mask &= accel_available_mask;
                    break;
                }
            }

            time_delay_us(wait_per_loop);
            wait_counter++;
        }

    now = time_micros();
    _sensor_imu._delta_time = (now - _sensor_imu._last_sample_usec) * 1.0e-6f;
    _sensor_imu._last_sample_usec = now;

#if 0
    {
        static uint64_t delta_time_sum;
        static uint16_t counter;
        if (delta_time_sum == 0) {
            delta_time_sum = _sample_period_usec;
        }
        delta_time_sum += _delta_time * 1.0e6f;
        if (counter++ == 400) {
            counter = 0;
            hal.console->printf("now=%lu _delta_time_sum=%lu diff=%ld\n",
                                (unsigned long)now,
                                (unsigned long)delta_time_sum,
                                (long)(now - delta_time_sum));
        }
    }
#endif

    _sensor_imu._have_sample = true;
}

/*
  get delta angles
 */
bool sensor_imu_get_delta_angle(uint8_t i, Vector3f_t *delta_angle, float *delta_angle_dt)
{
    if (_sensor_imu._delta_angle_valid[i] && _sensor_imu._delta_angle_dt[i] > 0) {
        *delta_angle_dt = _sensor_imu._delta_angle_dt[i];
    } else {
        *delta_angle_dt = sensor_imu_get_delta_time();
    }
    *delta_angle_dt = MIN(*delta_angle_dt, _sensor_imu._loop_delta_t_max);

    if (_sensor_imu._delta_angle_valid[i]) {
        *delta_angle = _sensor_imu._delta_angle[i];
        return true;
    } else if (sensor_imu_get_gyro_health2(i)) {
        // provide delta angle from raw gyro, so we use the same code
        // at higher level
        vec3_mult(delta_angle, &(_sensor_imu._gyro[i]), sensor_imu_get_delta_time());
        return true;
    }
    return false;
}

/*
  get delta velocity if available
*/
bool sensor_imu_get_delta_velocity(uint8_t i, Vector3f_t *delta_velocity, float *delta_velocity_dt)
{
    if (_sensor_imu._delta_velocity_valid[i]) {
        *delta_velocity_dt = _sensor_imu._delta_velocity_dt[i];
    } else {
        *delta_velocity_dt = sensor_imu_get_delta_time();
    }
    *delta_velocity_dt = MIN(*delta_velocity_dt, _sensor_imu._loop_delta_t_max);

    if (_sensor_imu._delta_velocity_valid[i]) {
        *delta_velocity = _sensor_imu._delta_velocity[i];
        return true;
    } else if (sensor_imu_get_accel_health2(i)) {
        // provide delta angle from raw gyro, so we use the same code
        // at higher level
        vec3_mult(delta_velocity, &(_sensor_imu._accel[i]), sensor_imu_get_delta_time());
        return true;
    }
    return false;
}


/* Find the N instance of the backend that has already been successfully detected */
sensor_imu_backend *sensor_imu_find_backend(int16_t backend_id, uint8_t instance)
{
    uint8_t found = 0;

    for (uint8_t i = 0; i < _sensor_imu._backend_count; i++) {
        int16_t id = sensor_imu_backend_get_id(_sensor_imu._backends[i]);

        if (id < 0 || id != backend_id) {
            continue;
        }

        if (instance == found) {
            return _sensor_imu._backends[i];
        } else {
            found++;
        }
    }

    return NULL;
}

bool sensor_imu_add_backend(sensor_imu_backend *backend)
{
    if (!backend) {
        return false;
    }
    if (_sensor_imu._backend_count == INS_MAX_BACKENDS) {
        console_panic("Too many INS backends");
    }
    _sensor_imu._backends[_sensor_imu._backend_count++] = backend;
    return true;
}

/*
  detect available backends for this board
 */
void sensor_imu_detect_backends()
{
    if (_sensor_imu._backends_detected) {
        return;
    }

    _sensor_imu._backends_detected = true;

    uint8_t probe_count = 0;
    uint8_t enable_mask = (uint8_t)_sensor_imu._enable_mask;
    uint8_t found_mask = 0;

    /*
      use ADD_BACKEND() macro to allow for INS_ENABLE_MASK for enabling/disabling INS backends
     */
#define ADD_BACKEND(x) do { \
        if (((1U<<probe_count)&enable_mask) && sensor_imu_add_backend(x)) { \
            found_mask |= (1U<<probe_count); \
        } \
        probe_count++; \
} while (0)

#if GP_FEATURE_BOARD_DETECT
    switch (brd_get_board_type()) {
    case PX4_BOARD_PIXHAWK:
        ADD_BACKEND(sensor_imu_invensense_probe(&_sensor_imu, devmgr_get_device(MPU60x0_SPI_DEVICE_NAME), ROTATION_ROLL_180));
        break;

    default:
        break;
    }
#else
    #error Unrecognised HAL_INS_TYPE setting
#endif

    if (_sensor_imu._backend_count == 0) {
        brd_config_error("INS: unable to initialise driver");
    }
}

// output GCS startup messages
bool sensor_imu_get_output_banner(uint8_t backend_id, char* banner, uint8_t banner_len)
{
    if (backend_id >= INS_MAX_BACKENDS || _sensor_imu._backends[backend_id] == NULL) {
        return false;
    }

    return sensor_imu_backend_get_output_banner(_sensor_imu._backends[backend_id], banner, banner_len);
}

// accelerometer clipping reporting
uint32_t sensor_imu_get_accel_clip_count(uint8_t instance)
{
    if (instance >= _sensor_imu._accel_count) {
        return 0;
    }
    return _sensor_imu._accel_clip_count[instance];
}

// get_gyro_health_all - return true if all gyros are healthy
bool sensor_imu_get_gyro_health_all(void)
{
    for (uint8_t i=0; i<_sensor_imu._gyro_count; i++) {
        if (!sensor_imu_get_gyro_health2(i)) {
            return false;
        }
    }
    // return true if we have at least one gyro
    return (_sensor_imu._gyro_count > 0);
}

// gyro_calibration_ok_all - returns true if all gyros were calibrated successfully
bool sensor_imu_gyro_calibrated_ok_all()
{
    for (uint8_t i=0; i<_sensor_imu._gyro_count; i++) {
        if (!_sensor_imu._gyro_cal_ok[i]) {
            return false;
        }
    }
    for (uint8_t i=_sensor_imu._gyro_count; i<INS_MAX_INSTANCES; i++) {
        if (_sensor_imu._gyro_id[i] != 0) {
            // missing gyro
            return false;
        }
    }
    return (_sensor_imu._gyro_count > 0);
}

// return true if gyro instance should be used (must be healthy and have it's use parameter set to 1)
bool sensor_imu_use_gyro(uint8_t instance)
{
    if (instance >= INS_MAX_INSTANCES) {
        return false;
    }

    return (sensor_imu_get_gyro_health2(instance) && _sensor_imu._use[instance]);
}

// get_accel_health_all - return true if all accels are healthy
bool sensor_imu_get_accel_health_all(void)
{
    for (uint8_t i=0; i<_sensor_imu._accel_count; i++) {
        if (!sensor_imu_get_accel_health2(i)) {
            return false;
        }
    }
    // return true if we have at least one accel
    return (_sensor_imu._accel_count > 0);
}

/*
  calculate the trim_roll and trim_pitch. This is used for redoing the
  trim without needing a full accel cal
 */
bool sensor_imu_calibrate_trim(Vector3f_t *trim_rad)
{
    Vector3f_t level_sample;

    // exit immediately if calibration is already in progress
    if (sensor_imu_calibrating()) {
        return false;
    }

    const uint8_t update_dt_milliseconds = (uint8_t)(1000.0f/_sensor_imu._loop_rate + 0.5f);

    // wait 100ms for ins filter to rise
    for (uint8_t k=0; k<100/update_dt_milliseconds; k++) {
        sensor_imu_wait_for_sample();
        sensor_imu_update();
        rt_thread_mdelay(update_dt_milliseconds);
    }

    uint32_t num_samples = 0;
    while (num_samples < 400/update_dt_milliseconds) {
        sensor_imu_wait_for_sample();
        // read samples from ins
        sensor_imu_update();
        // capture sample
        Vector3f_t samp;
        samp = _sensor_imu._accel[0];
        //level_sample += samp;
        vec3_add(&level_sample, &level_sample, &samp);
        if (!sensor_imu_get_accel_health2(0)) {
            return false;
        }
        rt_thread_mdelay(update_dt_milliseconds);
        num_samples++;
    }

    //level_sample /= num_samples;
    vec3_mult(&level_sample, &level_sample, 1.0f/num_samples);

    if (!_calculate_trim(&level_sample, trim_rad)) {
        return false;
    }

    return true;
}

/*
  check if the accelerometers are calibrated in 3D and that current number of accels matched number when calibrated
 */
bool sensor_imu_accel_calibrated_ok_all()
{
    // check each accelerometer has offsets saved
    for (uint8_t i=0; i<_sensor_imu._accel_count; i++) {
        if (!_sensor_imu._accel_id_ok[i]) {
            return false;
        }
        // exactly 0.0 offset is extremely unlikely
        if (vec3_is_zero(&(_sensor_imu._accel_offset[i]))) {
            return false;
        }
        // zero scaling also indicates not calibrated
        if (vec3_is_zero(&(_sensor_imu._accel_scale[i]))) {
            return false;
        }
    }
    for (uint8_t i=_sensor_imu._accel_count; i<INS_MAX_INSTANCES; i++) {
        if (_sensor_imu._accel_id[i] != 0) {
            // missing accel
            return false;
        }
    }
    
    // check calibrated accels matches number of accels (no unused accels should have offsets or scaling)
    if (_sensor_imu._accel_count < INS_MAX_INSTANCES) {
        for (uint8_t i=_sensor_imu._accel_count; i<INS_MAX_INSTANCES; i++) {
            const Vector3f_t *scaling = &(_sensor_imu._accel_scale[i]);
            bool have_scaling = (!math_flt_zero(scaling->x) && !math_flt_equal(scaling->x,1.0f)) || (!math_flt_zero(scaling->y) && !math_flt_equal(scaling->y,1.0f)) || (!math_flt_zero(scaling->z) && !math_flt_equal(scaling->z,1.0f));
            bool have_offsets = !vec3_is_zero(&(_sensor_imu._accel_offset[i]));
            if (have_scaling || have_offsets) {
                return false;
            }
        }
    }

    // if we got this far the accelerometers must have been calibrated
    return true;
}

// return true if accel instance should be used (must be healthy and have it's use parameter set to 1)
bool sensor_imu_use_accel(uint8_t instance)
{
    if (instance >= INS_MAX_INSTANCES) {
        return false;
    }

    return (sensor_imu_get_accel_health2(instance) && _sensor_imu._use[instance]);
}

// calculate vibration levels and check for accelerometer clipping (called by a backends)
void sensor_imu_calc_vibration_and_clipping(uint8_t instance, const Vector3f_t *accel, float dt)
{
    // check for clipping
    if (_sensor_imu._backends[instance] == NULL) {
        return;
    }
    if (fabsf(accel->x) >  _sensor_imu._backends[instance]->_clip_limit ||
        fabsf(accel->y) >  _sensor_imu._backends[instance]->_clip_limit ||
        fabsf(accel->z) >  _sensor_imu._backends[instance]->_clip_limit) {
        _sensor_imu._accel_clip_count[instance]++;
    }

    // calculate vibration levels
    if (instance < INS_VIBRATION_CHECK_INSTANCES) {
        // filter accel at 5hz
        Vector3f_t accel_filt = lpf_apply2_vec3f(&(_sensor_imu._accel_vibe_floor_filter[instance]), accel, dt);

        // calc difference from this sample and 5hz filtered value, square and filter at 2hz
        Vector3f_t accel_diff;
        vec3_sub(&accel_diff, accel, &accel_filt);

        accel_diff.x *= accel_diff.x;
        accel_diff.y *= accel_diff.y;
        accel_diff.z *= accel_diff.z;
        lpf_apply2_vec3f(&(_sensor_imu._accel_vibe_filter[instance]), &accel_diff, dt);
    }
}

// peak hold detector for slower mechanisms to detect spikes
void sensor_imu_set_accel_peak_hold(uint8_t instance, const Vector3f_t *accel)
{
    if (instance != _sensor_imu._primary_accel) {
        // we only record for primary accel
        return;
    }
    uint32_t now = time_millis();

    // negative x peak(min) hold detector
    if (accel->x < _sensor_imu._peak_hold_state.accel_peak_hold_neg_x ||
        _sensor_imu._peak_hold_state.accel_peak_hold_neg_x_age <= now) {
        _sensor_imu._peak_hold_state.accel_peak_hold_neg_x = accel->x;
        _sensor_imu._peak_hold_state.accel_peak_hold_neg_x_age = now + AP_INERTIAL_SENSOR_ACCEL_PEAK_DETECT_TIMEOUT_MS;
    }
}

// retrieve latest calculated vibration levels
Vector3f_t sensor_imu_get_vibration_levels2(uint8_t instance)
{
    Vector3f_t vibe;
    if (instance < INS_VIBRATION_CHECK_INSTANCES) {
        vibe = _sensor_imu._accel_vibe_filter[instance].output;
        vibe.x = math_sqrtf(vibe.x);
        vibe.y = math_sqrtf(vibe.y);
        vibe.z = math_sqrtf(vibe.z);
    }
    return vibe;
}

// retrieve latest calculated vibration levels
Vector3f_t sensor_imu_get_vibration_levels()
{
    return sensor_imu_get_vibration_levels2(_sensor_imu._primary_accel);
}

// check for vibration movement. Return true if all axis show nearly zero movement
bool sensor_imu_is_still()
{
    Vector3f_t vibe = sensor_imu_get_vibration_levels();
    return (vibe.x < _sensor_imu._still_threshold) &&
           (vibe.y < _sensor_imu._still_threshold) &&
           (vibe.z < _sensor_imu._still_threshold);
}

/*
  update IMU kill mask, used for testing IMU failover
 */
void sensor_imu_kill_imu(uint8_t imu_idx, bool kill_it)
{
    if (kill_it) {
        uint8_t new_kill_mask = _sensor_imu.imu_kill_mask | (1U<<imu_idx);
        // don't allow the last IMU to be killed
        bool all_dead = true;
        for (uint8_t i=0; i<MIN(_sensor_imu._gyro_count, _sensor_imu._accel_count); i++) {
            if (sensor_imu_use_gyro(i) && sensor_imu_use_accel(i) && !(new_kill_mask & (1U<<i))) {
                // we have at least one healthy IMU left
                all_dead = false;
            }
        }
        if (!all_dead) {
            _sensor_imu.imu_kill_mask = new_kill_mask;
        }
    } else {
        _sensor_imu.imu_kill_mask &= ~(1U<<imu_idx);
    }
}

/* get_delta_time returns the time period in seconds
 * overwhich the sensor data was collected
 */
float sensor_imu_get_delta_time() { return MIN(_sensor_imu._delta_time, _sensor_imu._loop_delta_t_max); }

// multi-device interface
inline bool sensor_imu_get_gyro_health2(uint8_t instance) { return (instance<_sensor_imu._gyro_count) ? _sensor_imu._gyro_healthy[instance] : false; }
inline bool sensor_imu_get_gyro_health(void) { return sensor_imu_get_gyro_health2(_sensor_imu._primary_gyro); }

inline bool sensor_imu_get_accel_health2(uint8_t instance) { return (instance<_sensor_imu._accel_count) ? _sensor_imu._accel_healthy[instance] : false; }
inline bool sensor_imu_get_accel_health(void) { return sensor_imu_get_accel_health2(_sensor_imu._primary_accel); }

static void _init_gyro()
{
    uint8_t num_gyros = MIN(_sensor_imu._gyro_count, INS_MAX_INSTANCES);
    Vector3f_t last_average[INS_MAX_INSTANCES], best_avg[INS_MAX_INSTANCES];
    Vector3f_t new_gyro_offset[INS_MAX_INSTANCES];
    float best_diff[INS_MAX_INSTANCES];
    bool converged[INS_MAX_INSTANCES];

    // exit immediately if calibration is already in progress
    if (sensor_imu_calibrating()) {
        return;
    }

    // record we are calibrating
    _sensor_imu._calibrating_gyro = true;

    // flash leds to tell user to keep the IMU still
    //AP_Notify::flags.initialising = true;

    // cold start
    console_printf("Init Gyro");

    /*
      we do the gyro calibration with no board rotation. This avoids
      having to rotate readings during the calibration
    */
    enum RotationEnum saved_orientation = _sensor_imu._board_orientation;
    _sensor_imu._board_orientation = ROTATION_NONE;

    // remove existing gyro offsets
    for (uint8_t k=0; k<num_gyros; k++) {
        vec3_zero(&(_sensor_imu._gyro_offset[k]));
        vec3_zero(&(new_gyro_offset[k]));
        best_diff[k] = -1.0f;
        vec3_zero(&(last_average[k]));
        converged[k] = false;
    }

    for(int8_t c = 0; c < 5; c++) {
        rt_thread_mdelay(5);
        sensor_imu_update();
    }

    // the strategy is to average 50 points over 0.5 seconds, then do it
    // again and see if the 2nd average is within a small margin of
    // the first

    uint8_t num_converged = 0;

    // we try to get a good calibration estimate for up to 30 seconds
    // if the gyros are stable, we should get it in 1 second
    for (int16_t j = 0; j <= 30*4 && num_converged < num_gyros; j++) {
        Vector3f_t gyro_sum[INS_MAX_INSTANCES], gyro_avg[INS_MAX_INSTANCES], gyro_diff[INS_MAX_INSTANCES];
        Vector3f_t accel_start;
        float diff_norm[INS_MAX_INSTANCES];
        uint8_t i;

        memset(diff_norm, 0, sizeof(diff_norm));

        console_printf("*");

        for (uint8_t k=0; k<num_gyros; k++) {
            vec3_zero(&(gyro_sum[k]));
        }
        accel_start = _sensor_imu._accel[0];
        for (i=0; i<50; i++) {
            sensor_imu_update();
            for (uint8_t k=0; k<num_gyros; k++) {
                vec3_add(&(gyro_sum[k]), &(gyro_sum[k]),  &(_sensor_imu._gyro[k]));
            }
            rt_thread_mdelay(5);
        }

        Vector3f_t accel_diff;
        vec3_sub(&accel_diff, &(_sensor_imu._accel[0]), &accel_start);
        if (vec3_length(&accel_diff) > 0.2f) {
            // the accelerometers changed during the gyro sum. Skip
            // this sample. This copes with doing gyro cal on a
            // steadily moving platform. The value 0.2 corresponds
            // with around 5 degrees/second of rotation.
            continue;
        }

        for (uint8_t k=0; k<num_gyros; k++) {
            //gyro_avg[k] = gyro_sum[k] / i;
            vec3_mult(&(gyro_avg[k]), &(gyro_sum[k]), 1/i);
            //gyro_diff[k] = last_average[k] - gyro_avg[k];
            vec3_sub(&(gyro_diff[k]), &(last_average[k]), &(gyro_avg[k]));
            //diff_norm[k] = gyro_diff[k].length();
            diff_norm[k] = vec3_length(&(gyro_diff[k]));
        }

        for (uint8_t k=0; k<num_gyros; k++) {
            if (best_diff[k] < 0) {
                best_diff[k] = diff_norm[k];
                best_avg[k] = gyro_avg[k];
            } else if (vec3_length(&(gyro_diff[k])) < ToRad(GYRO_INIT_MAX_DIFF_DPS)) {
                // we want the average to be within 0.1 bit, which is 0.04 degrees/s
                //last_average[k] = (gyro_avg[k] * 0.5f) + (last_average[k] * 0.5f);
                last_average[k] = vec3_mult_add(&(gyro_avg[k]), 0.5f, &(last_average[k]),  0.5f);
                if (!converged[k] || vec3_length(&(last_average[k])) < vec3_length(&(new_gyro_offset[k]))) {
                    new_gyro_offset[k] = last_average[k];
                }
                if (!converged[k]) {
                    converged[k] = true;
                    num_converged++;
                }
            } else if (diff_norm[k] < best_diff[k]) {
                best_diff[k] = diff_norm[k];
                //best_avg[k] = (gyro_avg[k] * 0.5f) + (last_average[k] * 0.5f);
                best_avg[k] = vec3_mult_add(&(gyro_avg[k]), 0.5f, &(last_average[k]),  0.5f);
            }
            last_average[k] = gyro_avg[k];
        }
    }

    // we've kept the user waiting long enough - use the best pair we
    // found so far
    console_printf("\n");
    for (uint8_t k=0; k<num_gyros; k++) {
        if (!converged[k]) {
            console_printf("gyro[%u] did not converge: diff=%f dps (expected < %f)\n",
                                (unsigned)k,
                                (double)ToDeg(best_diff[k]),
                                (double)GYRO_INIT_MAX_DIFF_DPS);
            _sensor_imu._gyro_offset[k] = best_avg[k];
            // flag calibration as failed for this gyro
            _sensor_imu._gyro_cal_ok[k] = false;
        } else {
            _sensor_imu._gyro_cal_ok[k] = true;
            _sensor_imu._gyro_offset[k] = new_gyro_offset[k];
        }
    }

    // restore orientation
    _sensor_imu._board_orientation = saved_orientation;

    // record calibration complete
    _sensor_imu._calibrating_gyro = false;

    // stop flashing leds
    //AP_Notify::flags.initialising = false;
}

/*
  _calculate_trim - calculates the x and y trim angles. The
  accel_sample must be correctly scaled, offset and oriented for the
  board

  Note that this only changes 2 axes of the trim vector. When in
  ROTATION_NONE view we can calculate the x and y trim. When in
  ROTATION_PITCH_90 for tailsitters we can calculate y and z. This
  allows users to trim for both flight orientations by doing two trim
  operations, one at each orientation

  When doing a full accel cal we pass in a trim vector that has been
  zeroed so the 3rd non-observable axis is reset
*/
static bool _calculate_trim(const Vector3f_t *accel_sample, Vector3f_t *trim)
{
    // allow multiple rotations, this allows us to cope with tailsitters
    // TOOD: 第二个旋转枚举是飞控安装方向
#if 0
        const enum Rotation rotations[] = {ROTATION_NONE,
#ifndef HAL_BUILD_AP_PERIPH
                                           AP::ahrs().get_view_rotation()
#endif
#endif
    const enum RotationEnum rotations[] = {ROTATION_NONE, ROTATION_NONE};
    bool good_trim = false;
    Vector3f_t newtrim;
    for (uint8_t i = 0; i < sizeof(rotations); i++) {
        newtrim = *trim;
        switch (rotations[i]) {
        case ROTATION_NONE:
            newtrim.y = atan2f(accel_sample->x, math_norm(accel_sample->y, accel_sample->z));
            newtrim.x = atan2f(-accel_sample->y, -accel_sample->z);
            break;

        case ROTATION_PITCH_90: {
            newtrim.y = atan2f(accel_sample->z, math_norm(accel_sample->y, -accel_sample->x));
            newtrim.z = atan2f(-accel_sample->y, accel_sample->x);
            break;
        }
        default:
            // unsupported
            continue;
        }
        if (fabsf(newtrim.x) <= radians(HAL_INS_TRIM_LIMIT_DEG) &&
            fabsf(newtrim.y) <= radians(HAL_INS_TRIM_LIMIT_DEG) &&
            fabsf(newtrim.z) <= radians(HAL_INS_TRIM_LIMIT_DEG)) {
            good_trim = true;
            break;
        }
    }
    if (!good_trim) {
        SENSOR_IMU_GCS_SEND_TEXT(MAV_SEVERITY_INFO, "trim over maximum of 10 degrees");
        return false;
    }
    *trim = newtrim;
    SENSOR_IMU_GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Trim OK: roll=%.2f pitch=%.2f yaw=%.2f",
                  (double)degrees(trim->x),
                  (double)degrees(trim->y),
                  (double)degrees(trim->z));
    return true;
}

/*------------------------------------test------------------------------------*/


